#include "NIJointManager.h"

inline bool IsEqualXnPoint3D(const XnPoint3D& lhs,const XnPoint3D& rhs)
{
	if(rhs.X==lhs.X && rhs.Y==lhs.Y && rhs.Z ==lhs.Z)return true;
	return false;
}

inline bool IsZeroXnPoint3D(const XnPoint3D& a)
{
	if(a.X>=-0.0001 && a.X<=0.0001)
	{
		if(a.Y>=-0.0001 && a.Y<=0.0001)
		{
			if(a.Z>=-0.0001 && a.Z<=0.0001)
			{
				return true;
			}
		}
	}
	return false;
}

inline bool IsEqual_X(const XnPoint3D& lhs,const XnPoint3D& rhs)
{
	if( rhs.X > lhs.X-JOINT3D_ERROR && rhs.X < lhs.X+JOINT3D_ERROR )
	{
		if(!IsZeroXnPoint3D(rhs)&&!IsZeroXnPoint3D(lhs))
		{
			return true;
		}
	}
	return false;
}

inline bool IsEqual_Vector3(const Vector3& lhs,const Vector3& rhs)
{
	if( rhs.x > lhs.x-Vector3_Error && rhs.x < lhs.x+Vector3_Error )
	{
		if( rhs.y > lhs.y-Vector3_Error && rhs.y < lhs.y+Vector3_Error )
		{
			if( rhs.z > lhs.z-Vector3_Error && rhs.z < lhs.z+Vector3_Error )
			{
				return true;
			}
		}
	}
	return false;
}

inline Vector3 XnPoint2Vector(XnPoint3D& lhs)
{
	return Vector3(lhs.X,lhs.Y,lhs.Z);
}

NIJointManager::NIJointManager( xn::SkeletonCapability& xnSC ):m_SC(xnSC)
{
	XnSkeletonJoint sk[] = 
	{
		XN_SKEL_HEAD,
		XN_SKEL_NECK,
		XN_SKEL_TORSO,
		XN_SKEL_LEFT_SHOULDER,
		XN_SKEL_LEFT_ELBOW,
		XN_SKEL_LEFT_HAND,
		XN_SKEL_RIGHT_SHOULDER,
		XN_SKEL_RIGHT_ELBOW,
		XN_SKEL_RIGHT_HAND,
		XN_SKEL_LEFT_HIP,
		XN_SKEL_LEFT_KNEE,
		XN_SKEL_LEFT_FOOT,
		XN_SKEL_RIGHT_HIP,
		XN_SKEL_RIGHT_KNEE,
		XN_SKEL_RIGHT_FOOT
	};
	for(int i=0;i<15;i++)
	{
		m_Joints.push_back(NIJoint(xnSC,sk[i],i));
	}
	delete []sk;
}

void NIJointManager::Update( XnUserID userID )
{
	for(int i=0;i<15;i++)
	{
		m_Joints[i].Update(userID);
	}
}

RotateParam NIJointManager::GetRotateParam( int idx1,int idx2 )
{
	Vector3 preJointVector;
	Vector3 curJointVector;
	RotateParam rp;
	Vector3 v1,v2;
	float d1,d2;
	// XnPoint to Vector3
	v1 = XnPoint2Vector(m_Joints[idx1].GetPrePoint());
	v2 = XnPoint2Vector(m_Joints[idx2].GetPrePoint());
	// v1 -----> v2
	preJointVector = v2 - v1;
	preJointVector.normalise();
	// XnPoint to Vector3
	v1 = XnPoint2Vector(m_Joints[idx1].GetCurPoint());
	v2 = XnPoint2Vector(m_Joints[idx2].GetCurPoint());
	//  v1 ----> v2
	curJointVector = v2 - v1;
	curJointVector.normalise();
	// Calculate two vector angleBetween
	d1 = preJointVector.angleBetween(Vector3::UNIT_X).valueDegrees();
	d2 = curJointVector.angleBetween(Vector3::UNIT_X).valueDegrees();
	//
	rp.degree = d2 - d1;
	rp.middle_up = curJointVector.crossProduct(preJointVector);
	rp.middle_up.normalise();
	return rp;
}
